function [trajectory, landmarks, stats] = run_graph_slam(true_traj, obstacles, dt, Q, R, laser_range, laser_fov, num_beams, laser_noise, show_progress)
% 运行Graph-SLAM算法
% 输入:
%   true_traj - 真实轨迹 [3 x steps]
%   obstacles - 障碍物位置 [N x 2]
%   dt - 时间步长
%   Q - 过程噪声协方差
%   R - 观测噪声协方差
%   laser_range - 激光最大距离
%   laser_fov - 激光视场角
%   num_beams - 激光束数量
%   laser_noise - 测距噪声标准差
%   show_progress - 是否显示进度（默认false）
% 输出:
%   trajectory - 优化后轨迹 [3 x steps]
%   landmarks - 路标Map
%   stats - 统计信息

    if nargin < 10
        show_progress = false;
    end
    
    steps = size(true_traj, 2);
    
    % 初始化位姿图
    pose_graph = struct();
    pose_graph.poses = [];
    pose_graph.edges = [];
    pose_graph.landmark_map = containers.Map('KeyType', 'int32', 'ValueType', 'any');
    
    % 数据收集
    progress_interval = max(1, round(steps/20));
    
    for t = 1:steps
        % 显示进度
        if show_progress && (mod(t, progress_interval) == 0 || t == 1 || t == steps)
            percent = round(100 * t / steps);
            n_landmarks = pose_graph.landmark_map.Count;
            n_edges = length(pose_graph.edges);
            fprintf('  [%3d%%] 步骤 %4d/%4d | 路标数: %3d | 边数: %4d\r', ...
                    percent, t, steps, n_landmarks, n_edges);
            if t == steps
                fprintf('\n');
            end
        end
        
        pose_graph.poses = [pose_graph.poses, true_traj(:, t)];
        
        % 里程计边
        if t > 1
            dx = true_traj(1, t) - true_traj(1, t-1);
            dy = true_traj(2, t) - true_traj(2, t-1);
            dtheta = wrapToPi(true_traj(3, t) - true_traj(3, t-1));
            v = sqrt(dx^2 + dy^2) / dt;
            omega = dtheta / dt;
            control = [v; omega];
            
            edge = struct();
            edge.type = 'odometry';
            edge.from = t - 1;
            edge.to = t;
            edge.pose_id = 0;
            edge.landmark_id = 0;
            edge.measurement = control;
            edge.dt = dt;
            edge.info = inv(Q(1:2, 1:2));
            pose_graph.edges = [pose_graph.edges; edge];
        end
        
        % 激光观测
        [observations, ~] = observation_model_laser(true_traj(:, t), obstacles, ...
                                        laser_range, laser_fov, num_beams, laser_noise);
        
        % 数据关联与添加观测边
        if ~isempty(observations)
            % 提取已知路标信息
            landmarks = [];
            landmark_ids = [];
            
            if pose_graph.landmark_map.Count > 0
                keys = pose_graph.landmark_map.keys();
                for k = 1:length(keys)
                    lm_id = keys{k};
                    landmark_ids = [landmark_ids; lm_id];
                    lm_idx = pose_graph.landmark_map(lm_id);
                    lm_pos = estimate_landmark_position(pose_graph, lm_idx, t);
                    landmarks = [landmarks; lm_pos'];
                end
            end
            
            % 数据关联
            [associations, is_new] = data_association(observations, true_traj(:, t), ...
                                                      landmarks, landmark_ids, [], R, []);
            
            % 添加观测边
            for i = 1:size(observations, 2)
                z = observations(:, i);
                
                if is_new(i)
                    % 新路标
                    new_id = pose_graph.landmark_map.Count + 1;
                    pose_graph.landmark_map(new_id) = new_id;
                    lm_idx = new_id;
                else
                    % 已知路标
                    assoc_idx = associations(i);
                    lm_id = landmark_ids(assoc_idx);
                    lm_idx = pose_graph.landmark_map(lm_id);
                end
                
                edge = struct();
                edge.type = 'landmark';
                edge.from = 0;
                edge.to = 0;
                edge.pose_id = t;
                edge.landmark_id = lm_idx;
                edge.measurement = z;
                edge.dt = 0;
                edge.info = inv(R);
                pose_graph.edges = [pose_graph.edges; edge];
            end
        end
    end
    
    % 图优化
    if show_progress
        fprintf('  正在优化位姿图...\n');
    end
    config = get_slam_config();
    optimized_graph = optimize_pose_graph(pose_graph, config.graph_slam.n_iterations);
    
    % 提取优化结果
    if isstruct(optimized_graph) && isfield(optimized_graph, 'poses')
        trajectory = optimized_graph.poses;
        landmarks = optimized_graph.landmark_map;
    else
        fprintf('  警告: 图优化失败，使用原始轨迹\n');
        trajectory = pose_graph.poses;
        landmarks = pose_graph.landmark_map;
    end
    
    % 计算统计信息
    position_error = sqrt(sum((true_traj(1:2, :) - trajectory(1:2, :)).^2, 1));
    stats = struct();
    stats.errors = position_error;
    stats.avg_error = mean(position_error);
    stats.max_error = max(position_error);
    stats.n_landmarks = landmarks.Count;
end
